A LCD Serial Terminal with Teensy

/*Serial terminal, print on a 40x4 LCD display,
  the library LiquidCrystalFast is used because it controls
  also display with double enable pins as that used
  Giovanni Carrera - 07/03/2017
*/
// include the library code:
#include <LiquidCrystalFast.h>
// set this to the hardware serial port used
#define HWSERIAL Serial1

const int Button = 22; // push button
const int DipSw1 = 23; // dip switch bit 1
const int DipSw2 = 24; // dip switch bit 2
const int DipSw3 = 25; // dip switch bit 3
const int DipSw4 = 26; // dip switch bit 4
const long baud_rate[9] = {1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200};
String linestr;
char displayline[4][41];// display buffers 4 rows x 40 characters
int r, i;
boolean ft_flag = true;// first time flag

// initialize the library with the numbers of the interface pins
//LiquidCrystalFast lcd(RS, RW, E1, E2, D4, D5, D6, D7);
LiquidCrystalFast lcd(21, 4, 2, 6, 17, 18, 19, 20);

void setup() {
  pinMode(DipSw1, INPUT); // set dip switch bits to input
  pinMode(DipSw2, INPUT);
  pinMode(DipSw3, INPUT);
  pinMode(DipSw4, INPUT);
  pinMode(Button, INPUT);
  // set up the LCD's number of rows and columns:
  lcd.begin(40, 4);
  // Print a message to the LCD.
  lcd.println("Serial terminal by GCar - 07/03/2017");
  // read dip switches for baud rate coding
  int brcode = digitalRead(DipSw1) + digitalRead(DipSw2) * 2 + digitalRead(DipSw3) * 4;
  lcd.print("Baud rate = ");
  lcd.println(baud_rate[brcode]);
  HWSERIAL.begin(baud_rate[brcode]);// for Teensy
  delay(5000);
  for (r = 0; r <= 3; r++) {
    displayline[r][0] = 0; // clear buffer
  }
  LCDprint(); // clear display
}

void LCDprint() {
  lcd.clear(); // clear diplay
  for (r = 0; r <= 3; r++) {
    lcd.setCursor(0, r); // set the cursor to column 0, line r
    lcd.print(displayline[r]); // print all four lines
  }
}

void loop() {
  r = 0;
  if (HWSERIAL.available() > 0) {
    linestr = HWSERIAL.readStringUntil('\n');
    if (ft_flag == true) {
      linestr.toCharArray(displayline[r], 41);// store line in buffer
      LCDprint();// print all four lines
      r++;
      if (r > 3) {
        r = 0;
        ft_flag = false;
      }
    }
    else {
      for (i = 0; i <= 40; i++) {
        displayline[0][i] = displayline[1][i];// scroll, line#0 = line#1
        displayline[1][i] = displayline[2][i];// scroll, line#1 = line#2
        displayline[2][i] = displayline[3][i];// scroll, line#2 = line#3
      }
      linestr.toCharArray(displayline[3], 41);// store new line in line#3
      LCDprint();// print all four lines
    }
  }
}